//
// Andy 2 firmware on Arduino Uno
//
// Coded by Yasuhiro ISHII
//

#include <MsTimer2.h>

#define PIN_MOTOR_R_VREF      5
#define PIN_MOTOR_L_VREF      6
#define PIN_MOTOR_R_CONTROL1  8
#define PIN_MOTOR_R_CONTROL2  9
#define PIN_MOTOR_L_CONTROL1  10
#define PIN_MOTOR_L_CONTROL2  11

#define MOTORTYPE_MOTOR_R    0
#define MOTORTYPE_MOTOR_L    1

#define MOTORACTION_MOTOR_STOP		0
#define MOTORACTION_MOTOR_FORWARD	1
#define MOTORACTION_MOTOR_REVERSE	2
#define MOTORACTION_MOTOR_BRAKE		3

#define MESSAGE_ACK		0x30
#define MESSAGE_ERROR		0x31
#define MESSAGE_NACK		0x32

#define MESSAGE_CATEGORY_INDICATOR_LED  0x10
#define MESSAGE_CATEGORY_CATERPILLAR	0x20
#define MESSAGE_CATEGORY_UNKNOWN	-1

#define SERIAL_MESSAGE_RECEIVE_MAX_LENGTH	100		/* Serial message max receive length [byte(s)] */
#define CYCLIC_HANDLER_INTERVAL_MS		100
#define SERIAL_COMMUNICATION_BAUD_RATE		115200
#define SERIAL_MESSAGE_RECEIVE_TIMEOUT_100MS	10		/* Timeout time for receiving command (10*100[ms]) */

typedef enum SERIAL_RECEIVE_STATE {
    SRECV_IDLE,
    SRECV_RECEIVING,
    SRECV_ERROR
} SerialReceiveState;

signed char motorPower_L;
signed char motorPower_R;
int serial_timeout_timer = 0;
int serial_timeout_detect = 0;

void setup()
{
    /////////////////////////////////////////////////
    // GPIO SETUP
    /////////////////////////////////////////////////

    // pin direction setup
    pinMode(2,OUTPUT);
    pinMode(3,OUTPUT);
    pinMode(4,OUTPUT);
    pinMode(5,OUTPUT);    // Vref (R ch)
    pinMode(6,OUTPUT);    // Vref (L ch)
    pinMode(7,OUTPUT);
  
    pinMode(8,OUTPUT);    // IN1 (Rch)
    pinMode(9,OUTPUT);    // IN2 (Rch)
    pinMode(10,OUTPUT);    // IN1 (Rch)
    pinMode(11,OUTPUT);    // IN2 (Rch)

    pinMode(12,OUTPUT);
    pinMode(13,OUTPUT);

    // pin output value setup
    digitalWrite(8,LOW);
    digitalWrite(9,LOW);
    digitalWrite(10,LOW);
    digitalWrite(11,LOW);


    /////////////////////////////////////////////////
    // MISC SETUP
    /////////////////////////////////////////////////

    // initialize peripherals
    analogWrite(PIN_MOTOR_R_VREF,0);
    analogWrite(PIN_MOTOR_L_VREF,0);

    motorPower_L = 0;
    motorPower_R = 0;

    // start cyclic handler
    MsTimer2::set(CYCLIC_HANDLER_INTERVAL_MS, cyclic_handler);
    MsTimer2::start();

    // start UART
    Serial.begin(SERIAL_COMMUNICATION_BAUD_RATE);
    Serial.flush();
}

void loop()
{
    static SerialReceiveState serial_receive_state = SRECV_IDLE;
    static int length = 0;
    static int category = -1;
    static int current_position = 0;
    static unsigned char command_buffer[SERIAL_MESSAGE_RECEIVE_MAX_LENGTH+1];
    int tmodet;

    noInterrupts();
    {
	tmodet = serial_timeout_detect;
    }
    interrupts();
    
    switch(serial_receive_state){
    case SRECV_IDLE:
	if(Serial.available() > 0){
	    length = Serial.read();
	    if(length < SERIAL_MESSAGE_RECEIVE_MAX_LENGTH){
		serial_receive_state = SRECV_RECEIVING;
		category = MESSAGE_CATEGORY_UNKNOWN;
		noInterrupts();
		{
		    serial_timeout_timer = SERIAL_MESSAGE_RECEIVE_TIMEOUT_100MS;
		    serial_timeout_detect = 0;
		}
		interrupts();
		current_position = 0;
	    } else {
		Serial.write(MESSAGE_ERROR);
	    }
	}
	break;
    case SRECV_RECEIVING:
	if(tmodet){
	    serial_receive_state = SRECV_ERROR;
	} else {
	    if(Serial.available() > 0){
		{
		    unsigned char readdata;
		    readdata = Serial.read();
		    if(current_position == 0){
			category = readdata;
		    } else {
			command_buffer[current_position] = readdata;
		    }
		}
		
		current_position++;
		if(current_position == length){

		    noInterrupts();
		    {
			serial_timeout_timer = 0;
		    }
		    interrupts();

		    switch(category){
		    case MESSAGE_CATEGORY_CATERPILLAR:
			noInterrupts();
			{
			    // process command
			    motorPower_L = command_buffer[1];
			    motorPower_R = command_buffer[2];
			}
			interrupts();
			
			// return ACK to the host
			Serial.write(MESSAGE_ACK);
			
			break;
		    case MESSAGE_CATEGORY_INDICATOR_LED:
			digitalWrite(13,command_buffer[1] == 0 ? LOW : HIGH);

			// return ACK to the host
			Serial.write(MESSAGE_ACK);
			break;
		    default:
			// command with unknown category is received
			
			Serial.write(MESSAGE_NACK);
			break;
		    }
		    serial_receive_state = SRECV_IDLE;
		}
	    }
	}
	
	break;
    case SRECV_ERROR:
	Serial.write(MESSAGE_ERROR);
	category = MESSAGE_CATEGORY_UNKNOWN;
	serial_receive_state = SRECV_IDLE;

	break;
    default:
	break;
    }
}

// for Debug
void led_indicator(void)
{
    static int flag = 0;
    
    if(flag){
	digitalWrite(13,LOW);
	flag = 0;
    } else {
	digitalWrite(13,HIGH);
	flag = 1;
    }
}

void serial_watcher(void)
{
    if(serial_timeout_timer != 0){
	serial_timeout_timer--;
	if(serial_timeout_timer == 0){
	    serial_timeout_detect = 1;
	    motorPower_L = 0;
	    motorPower_R = 0;
	}
    }
}

void setMotorSpeed(int power_l,int power_r)
{
    analogWrite(PIN_MOTOR_L_VREF,power_l);
    analogWrite(PIN_MOTOR_R_VREF,power_r);
    
    delay(10);
}

void controlMotor(int motortype,int motoraction)
{
    int pinno_1;
    int pinno_2;
    
    const unsigned char cont[4][2] = {
	{ LOW	,	LOW	},	// STOP
	{ HIGH	,	LOW	},	// FORWARD
	{ LOW	,	HIGH	},	// REVERSE
	{ HIGH	,	HIGH	}	// BRAKE
    };
    
    if(motortype == MOTORTYPE_MOTOR_R){
	pinno_1 = PIN_MOTOR_R_CONTROL1;
	pinno_2 = PIN_MOTOR_R_CONTROL2;
    } else {
	pinno_1 = PIN_MOTOR_L_CONTROL1;
	pinno_2 = PIN_MOTOR_L_CONTROL2;
    }
    
    digitalWrite(pinno_1,cont[motoraction][0]);
    digitalWrite(pinno_2,cont[motoraction][1]);  
}

void controlMotorProcessMain(void)
{
    // motorPower_L,motorPower_R : -128(rev max) ～ 0(stop) ～ +127(fwd max)
    int dirMotor_L;
    int dirMotor_R;
    
    if(!motorPower_L && !motorPower_R){
	setMotorSpeed(255,255);
	controlMotor(MOTORTYPE_MOTOR_R,MOTORACTION_MOTOR_BRAKE);
	controlMotor(MOTORTYPE_MOTOR_L,MOTORACTION_MOTOR_BRAKE);
    } else {
	dirMotor_L = motorPower_L < 0 ? MOTORACTION_MOTOR_REVERSE : MOTORACTION_MOTOR_FORWARD;
	dirMotor_R = motorPower_R < 0 ? MOTORACTION_MOTOR_REVERSE : MOTORACTION_MOTOR_FORWARD;
	
	controlMotor(MOTORTYPE_MOTOR_R,dirMotor_R);
	controlMotor(MOTORTYPE_MOTOR_L,dirMotor_L);
	setMotorSpeed(abs(motorPower_L) << 1,abs(motorPower_R) << 1);
    }
}

void cyclic_handler(void)
{
    serial_watcher();
    controlMotorProcessMain(); 
}
